#include <rclcpp/rclcpp.hpp>
#include "unitree_go/msg/sport_mode_state.hpp"
#include <fstream>
#include <iomanip>
#include <chrono>

class SportStateLogger : public rclcpp::Node
{
public:
    SportStateLogger() : Node("sport_state_logger")
    {
        // 打开日志文件
        log_file_.open("sport_state_log.csv", std::ios::out | std::ios::app);
        if (!log_file_.is_open()) {
            RCLCPP_ERROR(this->get_logger(), "无法打开日志文件");
        } else {
            log_file_ << "timestamp,vx,vy,vyaw\n";
        }
        
        sub_ = this->create_subscription<unitree_go::msg::SportModeState>(
            "/sportmodestate", 10,
            std::bind(&SportStateLogger::callback, this, std::placeholders::_1));
        RCLCPP_INFO(this->get_logger(), "运动状态日志节点已启动，订阅/sportstate");
    }

    ~SportStateLogger()
    {
        if (log_file_.is_open()) {
            log_file_.close();
        }
    }

private:
    void callback(const unitree_go::msg::SportModeState::SharedPtr msg)
    {
        // 获取当前时间戳
        auto now = this->now();
        double timestamp = now.seconds();
        float vx = msg->velocity[0];
        float vy = msg->velocity[1];
        float vyaw = msg->yaw_speed;
        
        // 写入日志
        if (log_file_.is_open()) {
            log_file_ << std::fixed << std::setprecision(6)
                      << timestamp << ","
                      << vx << "," << vy << "," << vyaw << "\n";
        }
        RCLCPP_INFO(this->get_logger(), "记录: vx=%.3f, vy=%.3f, vyaw=%.3f", vx, vy, vyaw);
    }

    std::ofstream log_file_;
    rclcpp::Subscription<unitree_go::msg::SportModeState>::SharedPtr sub_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SportStateLogger>());
    rclcpp::shutdown();
    return 0;
}
